/**
 * @file:          Fhan.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.10.16
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.10.16,18:39:18
 */

/* Include Files **************************************************************/
#include "Fhan.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/

void Fhan_Init(struct Fhan *self)
{
    self->Run = Fhan_Run_Normal;
    self->UpdateParam = Fhan_UpdateParam_Normal;

    self->UpdateParam(self);
}


float Fhan_Run_Normal(struct Fhan *self)
{
    if (self->param_en != 0)
    {
        /* method 2 */
        self->y = self->in_x1 + self->param_h * self->in_x2;
        float y_abs = fabsf(self->y);
        // self->a0 = bm_sqrt_carmack(self->d*self->d + 8*self->r*y_abs);
        self->a0 = bm_sqrt_carmack(self->d_squre + self->_8_r*y_abs);

        if (y_abs > self->d0)
        {
            self->a = self->in_x2 + (self->a0-self->d)*0.5F*sign_bm(self->y);
            // self->a = self->in_x2 + (self->a0*0.5F-self->d_div_2)*sign_bm(self->y);
        }else{
            self->a = self->in_x2 + self->y/self->param_h;
        }
        
        if (fabsf(self->a) > self->d)
        {
            self->out_u = -self->r*sign_bm(self->a);
        }else{
            // self->out_u = -self->r * self->a / self->d;
            self->out_u = self->neg_r_div_d * self->a;
        }
        
    }
    else
    {
        self->out_u = 0;
    }
    return self->out_u;
}

float Fhan_Run_Fsg(struct Fhan *self)
{
    if (self->param_en != 0)
    {
        self->a0 = self->param_h * self->in_x2;
        self->y = self->in_x1 + self->a0;

        self->a1 = bm_sqrt_carmack(self->d * (self->d + 8 * fabsf(self->y)));
        self->a2 = self->a0 + sign_bm(self->y) * (self->a1 - self->d) / 2.0F;

        float fsg_y_d = fsg_bm(self->y, self->d);
        float fsg_a_d = fsg_bm(self->a, self->d);
        self->a = (self->a0 + self->y) * fsg_y_d + self->a2 * (1 - fsg_y_d);
        self->out_u = -self->r * self->a / self->d * fsg_a_d - self->r * sign_bm(self->a) * (1 - fsg_a_d);
    }
    else
    {
        self->out_u = 0;
    }
    return self->out_u;
}

void Fhan_UpdateParam_Normal(struct Fhan *self)
{
    self->w0 = BM_2PI*self->param_f0;
    self->r = squre(self->w0/1.14F);
    self->d = self->r * self->param_h;
    self->d0 = self->d * self->param_h;

    self->d_squre = self->d * self->d;
    self->_8_r = 8.0F*self->r;
    self->neg_r_div_d = -self->r / self->d;
    self->d_div_2 = self->d / 2;
}


void Fhan_UpdateParam_Fsg(struct Fhan *self)
{
    self->w0 = BM_2PI*self->param_f0;
    self->r = squre(self->w0/1.14F);
    self->d = self->r * self->param_h * self->param_h;
}